
/**
 * From QT
 */

#include "math3d.h"

M3D_BEGIN_NAMESPACE


/************************************************************/
/***********************MVector3D****************************/
/***********************************************************/

MVector3D MVector3D::normalized() const
{
    // Need some extra precision if the length is very small.
    double len = double(xp) * double(xp) +
                 double(yp) * double(yp) +
                 double(zp) * double(zp);
    if (mFuzzyIsNull(len - 1.0f))
        return *this;
    else if (!mFuzzyIsNull(len)) 
        return div((*this), mSqrt(len));
    else
        return MVector3D();
}


void MVector3D::normalize()
{
    // Need some extra precision if the length is very small.
    double len = double(xp) * double(xp) +
                 double(yp) * double(yp) +
                 double(zp) * double(zp);
    if (mFuzzyIsNull(len - 1.0f) || mFuzzyIsNull(len))
        return;

    len = mSqrt(len);

    xp /= len;
    yp /= len;
    zp /= len;
}

mdouble MVector3D::dotProduct(const MVector3D& v1, const MVector3D& v2)
{
    return v1.xp * v2.xp + v1.yp * v2.yp + v1.zp * v2.zp;
}

MVector3D MVector3D::crossProduct(const MVector3D& v1, const MVector3D& v2)
{
    return MVector3D(v1.yp * v2.zp - v1.zp * v2.yp,
                    v1.zp * v2.xp - v1.xp * v2.zp,
                    v1.xp * v2.yp - v1.yp * v2.xp, 1);
}


/************************************************************/
/***********************MQuaternion*************************/
/***********************************************************/

MQuaternion MQuaternion::normalized() const
{

    // Need some extra precision if the length is very small.
    double len = double(xp) * double(xp) +
                 double(yp) * double(yp) +
                 double(zp) * double(zp) +
                 double(wp) * double(wp);
    if (mFuzzyIsNull(len - 1.0f))
        return *this;
    else if (!mFuzzyIsNull(len))
        return div((*this), mSqrt(len));
    else
        return MQuaternion(0.0f, 0.0f, 0.0f, 0.0f);

}   


MQuaternion MQuaternion::fromAxisAndAngle(const MVector3D& axis, mdouble angle)
{
    // Algorithm from:
    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
    // We normalize the result just in case the values are close
    // to zero, as suggested in the above FAQ.
    mdouble a = (angle / 2.0f) * M_PI / 180.0f;
    mdouble s = mSin(a);
    mdouble c = mCos(a);
    MVector3D ax = axis.normalized();

    return MQuaternion(c, ax.x() * s, ax.y() * s, ax.z() * s).normalized();
}

void MMatrix4x4::scale(mdouble x, mdouble y)
{
    if (flagBits == Identity) {
        m[0][0] = x;
        m[1][1] = y;
        flagBits = Scale;
    } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
        m[0][0] *= x;
        m[1][1] *= y;
    } else if (flagBits == Translation) {
        m[0][0] = x;
        m[1][1] = y;
        flagBits |= Scale;
    } else {
        m[0][0] *= x;
        m[0][1] *= x;
        m[0][2] *= x;
        m[0][3] *= x;
        m[1][0] *= y;
        m[1][1] *= y;
        m[1][2] *= y;
        m[1][3] *= y;
        flagBits = General;
    }
}

void MMatrix4x4::scale(mdouble x, mdouble y, mdouble z)
{
    if (flagBits == Identity) {
        m[0][0] = x;
        m[1][1] = y;
        m[2][2] = z;
        flagBits = Scale;
    } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
        m[0][0] *= x;
        m[1][1] *= y;
        m[2][2] *= z;
    } else if (flagBits == Translation) {
        m[0][0] = x;
        m[1][1] = y;
        m[2][2] = z;
        flagBits |= Scale;
    } else {
        m[0][0] *= x;
        m[0][1] *= x;
        m[0][2] *= x;
        m[0][3] *= x;
        m[1][0] *= y;
        m[1][1] *= y;
        m[1][2] *= y;
        m[1][3] *= y;
        m[2][0] *= z;
        m[2][1] *= z;
        m[2][2] *= z;
        m[2][3] *= z;
        flagBits = General;
    }
}

void MMatrix4x4::scale(mdouble factor)
{
    if (flagBits == Identity) {
        m[0][0] = factor;
        m[1][1] = factor;
        m[2][2] = factor;
        flagBits = Scale;
    } else if (flagBits == Scale || flagBits == (Scale | Translation)) {
        m[0][0] *= factor;
        m[1][1] *= factor;
        m[2][2] *= factor;
    } else if (flagBits == Translation) {
        m[0][0] = factor;
        m[1][1] = factor;
        m[2][2] = factor;
        flagBits |= Scale;
    } else {
        m[0][0] *= factor;
        m[0][1] *= factor;
        m[0][2] *= factor;
        m[0][3] *= factor;
        m[1][0] *= factor;
        m[1][1] *= factor;
        m[1][2] *= factor;
        m[1][3] *= factor;
        m[2][0] *= factor;
        m[2][1] *= factor;
        m[2][2] *= factor;
        m[2][3] *= factor;
        flagBits = General;
    }
}


void MMatrix4x4::translate(mdouble x, mdouble y)
{
    if (flagBits == Identity) {
        m[3][0] = x;
        m[3][1] = y;
        flagBits = Translation;
    } else if (flagBits == Translation) {
        m[3][0] += x;
        m[3][1] += y;
    } else if (flagBits == Scale) {
        m[3][0] = m[0][0] * x;
        m[3][1] = m[1][1] * y;
        m[3][2] = 0.;
        flagBits |= Translation;
    } else if (flagBits == (Scale | Translation)) {
        m[3][0] += m[0][0] * x;
        m[3][1] += m[1][1] * y;
    } else {
        m[3][0] += m[0][0] * x + m[1][0] * y;
        m[3][1] += m[0][1] * x + m[1][1] * y;
        m[3][2] += m[0][2] * x + m[1][2] * y;
        m[3][3] += m[0][3] * x + m[1][3] * y;
        if (flagBits == Rotation)
            flagBits |= Translation;
        else if (flagBits != (Rotation | Translation))
            flagBits = General;
    }
}

void MMatrix4x4::translate(mdouble x, mdouble y, mdouble z)
{
    if (flagBits == Identity) {
        m[3][0] = x;
        m[3][1] = y;
        m[3][2] = z;
        flagBits = Translation;
    } else if (flagBits == Translation) {
        m[3][0] += x;
        m[3][1] += y;
        m[3][2] += z;
    } else if (flagBits == Scale) {
        m[3][0] = m[0][0] * x;
        m[3][1] = m[1][1] * y;
        m[3][2] = m[2][2] * z;
        flagBits |= Translation;
    } else if (flagBits == (Scale | Translation)) {
        m[3][0] += m[0][0] * x;
        m[3][1] += m[1][1] * y;
        m[3][2] += m[2][2] * z;
    } else {
        m[3][0] += m[0][0] * x + m[1][0] * y + m[2][0] * z;
        m[3][1] += m[0][1] * x + m[1][1] * y + m[2][1] * z;
        m[3][2] += m[0][2] * x + m[1][2] * y + m[2][2] * z;
        m[3][3] += m[0][3] * x + m[1][3] * y + m[2][3] * z;
        if (flagBits == Rotation)
            flagBits |= Translation;
        else if (flagBits != (Rotation | Translation))
            flagBits = General;
    }
}

void MMatrix4x4::rotate(mdouble angle, mdouble x, mdouble y, mdouble z)
{
    if (angle == 0.0f)
        return;
    MMatrix4x4 m(1); // The "1" says to not load the identity.
    mdouble c, s, ic;
    if (angle == 90.0f || angle == -270.0f) {
        s = 1.0f;
        c = 0.0f;
    } else if (angle == -90.0f || angle == 270.0f) {
        s = -1.0f;
        c = 0.0f;
    } else if (angle == 180.0f || angle == -180.0f) {
        s = 0.0f;
        c = -1.0f;
    } else {
        mdouble a = angle * M_PI / 180.0f;
        c = mCos(a);
        s = mSin(a);
    }
    bool quick = false;
    if (x == 0.0f) {
        if (y == 0.0f) {
            if (z != 0.0f) {
                // Rotate around the Z axis.
                m.setToIdentity();
                m.m[0][0] = c;
                m.m[1][1] = c;
                if (z < 0.0f) {
                    m.m[1][0] = s;
                    m.m[0][1] = -s;
                } else {
                    m.m[1][0] = -s;
                    m.m[0][1] = s;
                }
                m.flagBits = General;
                quick = true;
            }
        } else if (z == 0.0f) {
            // Rotate around the Y axis.
            m.setToIdentity();
            m.m[0][0] = c;
            m.m[2][2] = c;
            if (y < 0.0f) {
                m.m[2][0] = -s;
                m.m[0][2] = s;
            } else {
                m.m[2][0] = s;
                m.m[0][2] = -s;
            }
            m.flagBits = General;
            quick = true;
        }
    } else if (y == 0.0f && z == 0.0f) {
        // Rotate around the X axis.
        m.setToIdentity();
        m.m[1][1] = c;
        m.m[2][2] = c;
        if (x < 0.0f) {
            m.m[2][1] = s;
            m.m[1][2] = -s;
        } else {
            m.m[2][1] = -s;
            m.m[1][2] = s;
        }
        m.flagBits = General;
        quick = true;
    }
    if (!quick) {
        mdouble len = x * x + y * y + z * z;
        if (!mFuzzyIsNull(len - 1.0f) && !mFuzzyIsNull(len)) {
            len = mSqrt(len);
            x /= len;
            y /= len;
            z /= len;
        }
        ic = 1.0f - c;
        m.m[0][0] = x * x * ic + c;
        m.m[1][0] = x * y * ic - z * s;
        m.m[2][0] = x * z * ic + y * s;
        m.m[3][0] = 0.0f;
        m.m[0][1] = y * x * ic + z * s;
        m.m[1][1] = y * y * ic + c;
        m.m[2][1] = y * z * ic - x * s;
        m.m[3][1] = 0.0f;
        m.m[0][2] = x * z * ic - y * s;
        m.m[1][2] = y * z * ic + x * s;
        m.m[2][2] = z * z * ic + c;
        m.m[3][2] = 0.0f;
        m.m[0][3] = 0.0f;
        m.m[1][3] = 0.0f;
        m.m[2][3] = 0.0f;
        m.m[3][3] = 1.0f;
    }
    int flags = flagBits;
    *this *= m;
    if (flags != Identity)
        flagBits = flags | Rotation;
    else
        flagBits = Rotation;
}



void MMatrix4x4::rotate(const MQuaternion& quaternion)
{
    // Algorithm from:
    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54
    MMatrix4x4 m(1);
    mdouble xx = quaternion.x() * quaternion.x();
    mdouble xy = quaternion.x() * quaternion.y();
    mdouble xz = quaternion.x() * quaternion.z();
    mdouble xw = quaternion.x() * quaternion.scalar();
    mdouble yy = quaternion.y() * quaternion.y();
    mdouble yz = quaternion.y() * quaternion.z();
    mdouble yw = quaternion.y() * quaternion.scalar();
    mdouble zz = quaternion.z() * quaternion.z();
    mdouble zw = quaternion.z() * quaternion.scalar();
    m.m[0][0] = 1.0f - 2 * (yy + zz);
    m.m[1][0] =        2 * (xy - zw);
    m.m[2][0] =        2 * (xz + yw);
    m.m[3][0] = 0.0f;
    m.m[0][1] =        2 * (xy + zw);
    m.m[1][1] = 1.0f - 2 * (xx + zz);
    m.m[2][1] =        2 * (yz - xw);
    m.m[3][1] = 0.0f;
    m.m[0][2] =        2 * (xz - yw);
    m.m[1][2] =        2 * (yz + xw);
    m.m[2][2] = 1.0f - 2 * (xx + yy);
    m.m[3][2] = 0.0f;
    m.m[0][3] = 0.0f;
    m.m[1][3] = 0.0f;
    m.m[2][3] = 0.0f;
    m.m[3][3] = 1.0f;
    int flags = flagBits;
    *this *= m;
    if (flags != Identity)
        flagBits = flags | Rotation;
    else
        flagBits = Rotation;
}





M3D_END_NAMESPACE
